#include "geomchecker.h"
#include "hedmodel.h"
#include "point.h"
#include "vertex.h"
#include "edge.h"
#include <iostream>
#include <math.h>

class Vertex;

GeomChecker::GeomChecker()
{

}



bool 
GeomChecker::hasAnyVertex( HEDModel& _model)
{
  int numVtx = _model.getNumberOfVertexes();
  if( numVtx > 0 )
    return true;
  return false;
}

bool 
GeomChecker::hasVertexWithGivenCoords( HEDModel& _model, int& id, double& x, double& y, double& z, double _tol )
{
  Vertex* v = _model.getVertex(x,y,z,_tol);
  if( v != NULL )
  {
    id = v->getVertexno();
    x = v->getPoint()->getX();
    y = v->getPoint()->getY();
    z = v->getPoint()->getZ();
    return true;
  }
  return false;
}


bool 
GeomChecker::liesOnCurve( HEDModel& _model, int& id, double& x, double& y, double& z, double& _tol )
{
  Edge* edg = _model.getEdge(x,y,z,_tol);
  if( edg != NULL )
  {
    Point* pt1 = edg->getFirstVertex()->getPoint();
    Point* pt2 = edg->getSeconVertex()->getPoint();
    double vx = pt2->getX() - pt1->getX();
    double vy = pt2->getY() - pt1->getY();
    double t = (vx*(x-pt1->getX()) + vy*(y-pt1->getY())) / (vx*vx + vy*vy);
    x = pt1->getX() + t * vx;
    y = pt1->getY() + t * vy;
    //id = edg->getEdgeno();
    return true;
  }
  return false;
}


bool 
GeomChecker::intersectsVertex( HEDModel& _model, double* curvePts, int& n, double* intersPts )
{
  vector<Vertex*> vertices = _model.getVertexes();
  for( size_t i=0; i< vertices.size(); i++ )
  {
    Point* pt = vertices[i]->getPoint();
    //double LineMag2 = _LineStart.distance2(_LineEnd);
    //if (LineMag2 == 0.0)
    //  return 0;
    //Point3D v1 = _Point   - _LineStart;
    //Point3D v2 = _LineEnd - _LineStart;
    //double U = (v1.dotProd (v2)) / LineMag2;
    //if( U < 0.0 || U > 1.0 )
    //  return 0; // closest point does not fall within the line segment
    //_Intersection = _LineStart + U * v2;
    //_d = _Point.distance(_Intersection);
  }
  return false;
}


bool 
GeomChecker::intersectsCurves( HEDModel& _model, Point& cp1, Point& cp2, list<Point>& intersPts, list<Edge*>& iEdges )
{
  double xi, yi;
  vector<Point> vInPts;
  vector<Edge*> vEdges;

  vector<Edge*> edges = _model.getEdges();
  for( size_t i=0; i< edges.size(); i++ )
  {
    Point* pt1 = edges[i]->getFirstVertex()->getPoint();
    Point* pt2 = edges[i]->getSeconVertex()->getPoint();
    int inters = this->computeLineLineIntersection( pt1->getX(), pt1->getY(), pt2->getX(), pt2->getY(),
                                                    cp1.getX(), cp1.getY(), cp2.getX(), cp2.getY(), xi, yi );
    if( inters )
    {
      Point interPt(xi,yi,0.0);
      vInPts.push_back( interPt );
      vEdges.push_back( edges[i] );
    }
  }

  // sorts the intersected points based on the distances to the curve first point
  int i = vInPts.size();
  if( i > 0 )
  {
    bool ordered = false;
    int k = i;
    while( (ordered == false) && (k > 1) )
    {
      ordered = true;
      int j = -1;
      int l = 0;
      while( l < i-1 )
      {
        j++;
        double distj = cp1.distance2( vInPts[j].getX(),vInPts[j].getY(),vInPts[j].getZ() );
        l++;
        double distm = cp1.distance2( vInPts[l].getX(),vInPts[l].getY(),vInPts[l].getZ() );
        if( distj > distm )
        {
          Point pt = vInPts[j];
          vInPts[j] = vInPts[l];
          vInPts[l] = pt;
          Edge* ed = vEdges[j];
          vEdges[j] = vEdges[l];
          vEdges[l] = ed;
          ordered = false;
        }
        k++;
      }
    }
    // stores the data from vectors in the lists
    for( size_t i=0; i< vInPts.size(); i++ )
    {
      intersPts.push_back( vInPts[i] );
      iEdges.push_back( vEdges[i] );
    }
    return true;
  }

  return false;
}




int 
GeomChecker::computeLineLineIntersection( double x1, double y1, double x2, double y2,
                                          double x3, double y3, double x4, double y4, double& xi, double& yi )
{
  double xD1,yD1,xD2,yD2,xD3,yD3;  
  double dot,deg,len1,len2;  
  double segmentLen1,segmentLen2;  
  double ua,ub,div;  

  // calculate differences  
  xD1=x2-x1;  
  xD2=x4-x3;  
  yD1=y2-y1;  
  yD2=y4-y3;  
  xD3=x1-x3;  
  yD3=y1-y3;    

  // calculate the lengths of the two lines  
  len1=sqrt(xD1*xD1+yD1*yD1);  
  len2=sqrt(xD2*xD2+yD2*yD2);  

  // calculate angle between the two lines.  
  dot=(xD1*xD2+yD1*yD2); // dot product  
  deg=dot/(len1*len2);  

  // if abs(angle)==1 then the lines are parallell,  
  // so no intersection is possible  
  if(fabs(deg)==1) return 0;

  // find intersection Pt between two lines  
  div=yD2*xD1-xD2*yD1;  
  ua=(xD2*yD3-yD2*xD3)/div;  
  ub=(xD1*yD3-yD1*xD3)/div;  
  double x=x1+ua*xD1;  
  double y=y1+ua*yD1;  

  // calculate the combined length of the two segments  
  // between Pt-p1 and Pt-p2  
  xD1=x-x1;  
  xD2=x-x2;  
  yD1=y-y1;  
  yD2=y-y2;  
  segmentLen1=sqrt(xD1*xD1+yD1*yD1)+sqrt(xD2*xD2+yD2*yD2);  

  // calculate the combined length of the two segments  
  // between Pt-p3 and Pt-p4  
  xD1=x-x3;  
  xD2=x-x4;  
  yD1=y-y3;  
  yD2=y-y4;  
  segmentLen2=sqrt(xD1*xD1+yD1*yD1)+sqrt(xD2*xD2+yD2*yD2);  

  // if the lengths of both sets of segments are the same as  
  // the lenghts of the two lines the point is actually  
  // on the line segment.  

  // if the point isn't on the line, return null  
  if(fabs(len1-segmentLen1)>0.01 || fabs(len2-segmentLen2)>0.01)
    return 0;  

  // return the valid intersection  
  xi = x;
  yi = y;
  return 1;
}


int 
GeomChecker::comp( double _a, double _b, double _tol )
{
  double delta = fabs( _a - _b );
  if( delta < _tol )  return 0;
  else if( _a > _b )  return 1;
  else                return -1;
}


int 
GeomChecker::contvv( double x1, double y1, double x2, double y2, double _tol )
{
  double dx = x1 - x2;
  double dy = y1 - y2;
  double diff = dx * dx + dy * dy;
  return ( this->comp( diff, 0.0, _tol*_tol ) );
}
